# from gpiozero import Motor 
from gpiozero import DigitalInputDevice

import sys
import select
from time import sleep
from signal import pause
from datetime import datetime
import warnings
import threading

# motor relay pacakge import 
from collections import OrderedDict

from gpiozero import SourceMixin, CompositeDevice
from gpiozero import PWMOutputDevice, DigitalOutputDevice, LED, Button


from gpiozero import (
    OutputDeviceBadValue,
    # GPIOPinMissing,
    # PWMSoftwareFallback,
    # DeviceClosed,
)

import os, time, uuid, hashlib, configparser, subprocess, re


VERSION = "1.0.0"

read_path = "/tmp/motor_pipe.in"
write_path = "/tmp/motor_pipe.out"

bj_sensor_status_path="/home/bj/tmp/gpio_status.txt"

DEBUG_PRINT = False

class MyMotor(SourceMixin, CompositeDevice):
    """
    Extends :class:`CompositeDevice` and represents a generic motor
    connected to a bi-directional motor driver circuit (i.e. an `H-bridge`_).

    Attach an `H-bridge`_ motor controller to your Pi; connect a power source
    (e.g. a battery pack or the 5V pin) to the controller; connect the outputs
    of the controller board to the two terminals of the motor; connect the
    inputs of the controller board to two GPIO pins.

    .. _H-bridge: https://en.wikipedia.org/wiki/H_bridge

    The following code will make the motor turn "forwards"::

        from gpiozero import Motor

        motor = Motor(17, 18)
        motor.forward()

    :type forward: int or str
    :param forward:
        The GPIO pin that the forward input of the motor driver chip is
        connected to. See :ref:`pin-numbering` for valid pin numbers. If this
        is :data:`None` a :exc:`GPIODeviceError` will be raised.

    :type backward: int or str
    :param backward:
        The GPIO pin that the backward input of the motor driver chip is
        connected to. See :ref:`pin-numbering` for valid pin numbers. If this
        is :data:`None` a :exc:`GPIODeviceError` will be raised.

    :type enable: int or str or None
    :param enable:
        The GPIO pin that enables the motor. Required for *some* motor
        controller boards. See :ref:`pin-numbering` for valid pin numbers.

    :param bool pwm:
        If :data:`True` (the default), construct :class:`PWMOutputDevice`
        instances for the motor controller pins, allowing both direction and
        variable speed control. If :data:`False`, construct
        :class:`DigitalOutputDevice` instances, allowing only direction
        control.

    :type pin_factory: Factory or None
    :param pin_factory:
        See :doc:`api_pins` for more information (this is an advanced feature
        which most users can ignore).
    """
    def __init__(self, forward, backward, *, enable=None, pwm=True,
                 pin_factory=None):
        PinClass = PWMOutputDevice if pwm else DigitalOutputDevice
        devices = OrderedDict((
            ('forward_device', PinClass(forward, pin_factory=pin_factory, frequency= 100)),
            ('backward_device', PinClass(backward, pin_factory=pin_factory, frequency= 100)),
        ))
        if enable is not None:
            devices['enable_device'] = DigitalOutputDevice(
                enable,
                initial_value=True,
                pin_factory=pin_factory
            )
        super().__init__(_order=devices.keys(), pin_factory=pin_factory, **devices)

    @property
    def value(self):
        """
        Represents the speed of the motor as a floating point value between -1
        (full speed backward) and 1 (full speed forward), with 0 representing
        stopped.
        """
        return self.forward_device.value - self.backward_device.value

    @value.setter
    def value(self, value):
        if not -1 <= value <= 1:
            raise OutputDeviceBadValue("Motor value must be between -1 and 1")
        if value > 0:
            try:
                self.forward(value)
            except ValueError as e:
                raise OutputDeviceBadValue(e)
        elif value < 0:
            try:
               self.backward(-value)
            except ValueError as e:
                raise OutputDeviceBadValue(e)
        else:
            self.stop()

    @property
    def is_active(self):
        """
        Returns :data:`True` if the motor is currently running and
        :data:`False` otherwise.
        """
        return self.value != 0

    def forward(self, speed=1):
        """
        Drive the motor forwards.

        :param float speed:
            The speed at which the motor should turn. Can be any value between
            0 (stopped) and the default 1 (maximum speed) if *pwm* was
            :data:`True` when the class was constructed (and only 0 or 1 if
            not).
        """
        if not 0 <= speed <= 1:
            raise ValueError('forward speed must be between 0 and 1')
        if isinstance(self.forward_device, DigitalOutputDevice):
            if speed not in (0, 1):
                raise ValueError(
                    'forward speed must be 0 or 1 with non-PWM Motors')
        self.backward_device.off()
        self.forward_device.value = speed

    def backward(self, speed=1):
        """
        Drive the motor backwards.

        :param float speed:
            The speed at which the motor should turn. Can be any value between
            0 (stopped) and the default 1 (maximum speed) if *pwm* was
            :data:`True` when the class was constructed (and only 0 or 1 if
            not).
        """
        if not 0 <= speed <= 1:
            raise ValueError('backward speed must be between 0 and 1')
        if isinstance(self.backward_device, DigitalOutputDevice):
            if speed not in (0, 1):
                raise ValueError(
                    'backward speed must be 0 or 1 with non-PWM Motors')
        self.forward_device.off()
        self.backward_device.value = speed

    def control(self, forward_speed = 0, backward_speed = 0):
        """_summary_

        Args:
            forward_speed (int, optional): _description_. Defaults to 0.
            speed (int, optional): _description_. Defaults to 1.
        """
        # if not 0.5 <= speed <= 1.0:
        #     raise ValueError('forward speed must be between 0.5 and 1')
        # if isinstance(self.forward_device, DigitalOutputDevice):
        #     if speed not in (0, 1):
        #         raise ValueError(
        #             'forward speed must be 0 or 1 with non-PWM Motors')
    
        self.backward_device.off()
        self.forward_device.off()
                
        self.forward_device.value = forward_speed
        self.backward_device.value = backward_speed
        

    def reverse(self):
        """
        Reverse the current direction of the motor. If the motor is currently
        idle this does nothing. Otherwise, the motor's direction will be
        reversed at the current speed.
        """
        self.value = -self.value

    def stop(self):
        """
        Stop the motor.
        """
        self.forward_device.off()
        self.backward_device.off()

    def brake(self, keep_speed = 0.2):
        
        """_summary_
        电机制动
        """
        self.forward_device.value = keep_speed
        self.backward_device.value = keep_speed
        
        time.sleep(1)
        
        self.forward_device.value = 0
        self.backward_device.value = 0


class Motor_Sensor():
    def __init__(self, name=None, pin=None, bounce_timeout=None):
        self.name = name
        print(f"name : {self.name}")
        
        self.sensor = DigitalInputDevice(pin, bounce_time=bounce_timeout)
        # self.sensor.when_activated = self.activated_func
        # self.sensor.when_deactivated = self.deactivated_func
    
    def activated_func(self):
        current_time = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
        print(f"[{current_time}]-[{self.name}] activated func: {self.sensor.value}")
        
    def deactivated_func(self):
        current_time = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
        print(f"[{current_time}]-[{self.name}] deactivated func: {self.sensor.value}")
    
    def value_print(self):
        current_time = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
        print(f"[{current_time}]-[{self.name}] value: {self.sensor.value}")
        

# 电机引脚
X_forward_pin = 22
X_backward_pin = 23
Y_forward_pin = 17
Y_backward_pin = 18

# 设置要读取的 GPIO 引脚
input_pin = 24          # 复位检测
angle_check_pin = 25    # 角度检测
sensor1_pin = 16        # 
sensor2_pin = 20
sensor3_pin = 21

# 2个指示灯 引脚
run_led_pin = 10
signal_led_pin = 9

# 复位按键引脚
reset_btn_pin = 11

# 五向按键引脚
U_btn_pin = 19
L_btn_pin = 13
D_btn_pin = 6
M_btn_pin = 5
R_btn_pin = 0

# motor_speed_base = 0.15
# motor_speed = 0.98       # 电机速度
motor_speed_base = 0
#motor_speed = 1       # 电机速度
motor_speed = 0.2

bounce_timeout = 0.03

# 初始化上一次触发时间为 None
last_triggered_time = None
wait_reset_flag = False
Y_forward_flag = False
angle_cnt = 0           # 记录当前角度计数
angle_cnt_2 = 0         # 记录旋转计数
angle_cnt_2_set = 0     # 记录设置计数

angle_check_flag = 0    # 角度检测标志

# 创建 DigitalInputDevice 对象
input_device = DigitalInputDevice(input_pin, bounce_time=bounce_timeout)
angle_check = DigitalInputDevice(angle_check_pin, bounce_time=bounce_timeout)
sensor1 = Motor_Sensor("sensor1", sensor1_pin, bounce_timeout)
sensor2 = Motor_Sensor("sensor2", sensor2_pin, bounce_timeout)
sensor3 = Motor_Sensor("sensor3", sensor3_pin, bounce_timeout)

# 创建 MyMotor 
motor_claw_X = MyMotor(forward=X_forward_pin, backward=X_backward_pin, pwm=True)
motor = MyMotor(forward=Y_forward_pin, backward=Y_backward_pin, pwm=True)

# 初始化指示灯和按键
run_led = LED(run_led_pin)
signal_led = LED(signal_led_pin)

reset_btn = Button(reset_btn_pin, bounce_time=bounce_timeout)
U_btn = Button(U_btn_pin, pull_up=False, bounce_time=bounce_timeout)
L_btn = Button(L_btn_pin, pull_up=False, bounce_time=bounce_timeout)
D_btn = Button(D_btn_pin, pull_up=False, bounce_time=bounce_timeout)
M_btn = Button(M_btn_pin, pull_up=False, bounce_time=bounce_timeout)
R_btn = Button(R_btn_pin, pull_up=False, bounce_time=bounce_timeout)



def print_active_state():
    global last_triggered_time
    global input_device
    global wait_reset_flag
    
    if wait_reset_flag == True:
        return
    
    # 获取当前触发时间
    current_time = datetime.now()
    
    # 如果是第一次触发，记录触发时间并返回
    if last_triggered_time is None:
        last_triggered_time = current_time
        return

    # 计算两次触发之间的时间间隔
    time_delta = current_time - last_triggered_time

    # 打印时间间隔和当前 GPIO 引脚的状态
    print("Time since last trigger:", time_delta)
    print(f"{current_time} - Input state: {input_device.value}")

    # 更新上一次触发时间为当前时间
    last_triggered_time = current_time

# Y轴 上电复位触发
def print_deactive_func():
    global motor
    global input_device
    global last_triggered_time
    global wait_reset_flag

    if wait_reset_flag == True:
        return


    if 1 == input_device.value:
        return

    # motor.stop()
    motor.brake(keep_speed=1)
    print("motor brake")
    print(f"motor value:{motor.value}")
    
    wait_reset_flag = True
    
    # 获取当前触发时间
    current_time = datetime.now()
    
    # 如果是第一次触发，记录触发时间并返回
    if last_triggered_time is None:
        last_triggered_time = current_time
        return
    
    # 计算两次触发之间的时间间隔
    time_delta = current_time - last_triggered_time

    # 打印时间间隔和当前 GPIO 引脚的状态
    print("Time since last trigger:", time_delta)
    print(f"{current_time} - Input state: {input_device.value}")
    
    # 更新上一次触发时间为当前时间
    last_triggered_time = current_time
    
    
    # sys.exit(0)

# 顺时针若第一次 1 0 1 0 :45度
# 顺时针后续 0 1 0 :45度

# 逆时针若第一次 0 1 0 1 :45度
# 逆时针后续  1 0 1:45度
def angle_active_func():
    global angle_check
    global angle_cnt
    global angle_cnt_2
    global angle_cnt_2_set
    global wait_reset_flag
    global angle_check_flag
    
    if wait_reset_flag == False:
        return
    
    # 逆时针
    if Y_forward_flag == False:
        if angle_check_flag == 0:
            angle_cnt_2 = angle_cnt_2 + 1

        # 1:45, 2:90 
        if angle_cnt_2 == angle_cnt_2_set:
            angle_cnt_2 = 0

            motor.brake(keep_speed=1)
            print("motor brake")
            print(f"ni stop angle_cnt:{angle_cnt}, angle:{angle_cnt * 45}")

    else:
        if angle_check_flag == 0:
            angle_cnt_2 = angle_cnt_2 + 1
        
        angle_cnt = angle_cnt + 1 # 顺时针角度加
        if angle_cnt >= 8:
            angle_cnt = 0
        #print(f"shun  angle_cnt value: {angle_cnt}")

    # 获取当前触发时间
    current_time = datetime.now()
    
    if DEBUG_PRINT == True:
        print(f"{current_time} - angle_active_func state: {angle_check.value}")

def angle_deactive_func():  
    global angle_check
    global angle_cnt
    global angle_cnt_2
    global wait_reset_flag
    global angle_check_flag
    
    if wait_reset_flag == False:
        return
    
    """
    顺时针遇到低电平为45度
    逆时针遇到高电平为45度
    """
    if Y_forward_flag == False:
        if angle_check_flag == 1:
            angle_cnt_2 = angle_cnt_2 + 1
        
        angle_cnt = angle_cnt - 1  # 逆时针角度减
        if angle_cnt < 0:
            angle_cnt = 7
            
        #print(f"ni angle_cnt:{angle_cnt}")
    else:
        # 1:45, 2:90 
        if angle_check_flag == 1:
            angle_cnt_2 = angle_cnt_2 + 1

        if angle_cnt_2 == angle_cnt_2_set:
            angle_cnt_2 = 0
            
            motor.brake(keep_speed=1)
            print("motor brake")
            print(f"shun stop angle_cnt:{angle_cnt}, angle:{angle_cnt * 45}")
            
        print(f"shun angle_cnt:{angle_cnt}")
    # 获取当前触发时间
    current_time = datetime.now()
    
    if DEBUG_PRINT == True:
        print(f"{current_time} - angle_deactive_func state: {angle_check.value}")


def X_claw_control(angle:int):
    """_summary_
    设置 X轴旋转角度
    Args:
        angle (int): _description_  0° or 90°
    """
    timeout_cnt = 0
    
    if angle == 0:
        print(f"wait sensor1 and sensor2-->:0")
        if sensor3.sensor.is_active == True and sensor2.sensor.is_active == True:
            return
         
        # motor_claw_X.control(0, 0.1)
        motor_claw_X.control(motor_speed_base, motor_speed)
        
        
        while sensor3.sensor.wait_for_active(0.1) != True:
            timeout_cnt = timeout_cnt + 1
            if timeout_cnt > 50:
                break
            
        sensor2.value_print()
        
        while sensor2.sensor.wait_for_active(0.1) != True:
            timeout_cnt = timeout_cnt + 1
            if timeout_cnt > 100:
                print("X_claw_control")
                break
            
        motor_claw_X.brake(keep_speed=1)

    elif angle == 90:
        # 顺时针
        print(f"wait sensor1 and sensor2-->90")
        if sensor1.sensor.is_active == True and sensor2.sensor.is_active == True:
            return
        
        # motor_claw_X.control(0.1, 0)
        motor_claw_X.control(motor_speed, motor_speed_base)
        
        # 爪子 90度 等待
        while sensor1.sensor.wait_for_active(0.1) != True:
            timeout_cnt = timeout_cnt + 1
            if timeout_cnt > 50:
                break
            
        sensor2.value_print()
            
        while sensor2.sensor.wait_for_inactive(0.1) != True:
            timeout_cnt = timeout_cnt + 1
            if timeout_cnt > 100:
                print("X_claw_control")
                break
            
        motor_claw_X.brake(keep_speed=1)


    if DEBUG_PRINT == True:
        print("=======================================")
        sensor1.value_print()
        sensor2.value_print()
        sensor3.value_print()
        print("=======================================")

def Y_spin_control(angle:int):
    """_summary_
    设置 Y轴旋转角度

    Args:
        angle (int): _description_
    """
    global angle_cnt
    global angle_cnt_2
    global angle_cnt_2_set
    global Y_forward_flag
    
    if angle < 0 or angle > 360:
        print("angle is [0, 360]")
        return
    
    timeout_cnt = 0
    angle_cnt_2 = 0
    t_forward_flag = True
    
    # 计算与当前位置的角度差
    #print(f"current angle:{angle_cnt * 45}")
    t_cnt = angle / 45
    if t_cnt > angle_cnt:
        t_cnt = t_cnt - angle_cnt
    elif t_cnt < angle_cnt:
        t_cnt = 8 - angle_cnt + t_cnt
    else:
        return
 
    
    angle_cnt_2_set = t_cnt

    if t_forward_flag == True:
        Y_forward_flag = True
        # motor.control(0.15, 0)
        motor.control(motor_speed, motor_speed_base)
    else:
        Y_forward_flag = False
        # motor.control(0, 0.15)
        motor.control(motor_speed_base, motor_speed)


def Y_spin_control_plus(angle:int):
    """_summary_
    设置 Y轴旋转角度

    Args:
        angle (int): _description_
    """
    global angle_cnt
    global angle_cnt_2
    global angle_cnt_2_set
    global Y_forward_flag
    global angle_check_flag

    if angle == 0:
        return

    angle_cnt_2 = 0
    t_forward_flag = True
    
    # 计算与当前位置的角度差
    #print(f"current angle:{angle_cnt * 45}")
    
    if angle < 0:
        t_forward_flag = False
        t_cnt = -angle / 45
    else:
        t_forward_flag = True
        t_cnt = angle /45

    angle_cnt_2_set = t_cnt
    # 逆时针 0 1 0 1
    if angle < 0 and angle_check_flag == 0:
        angle_cnt_2_set = angle_cnt_2_set + 1
        
    # 顺时针 1 0 1 0
    if angle > 0 and angle_check_flag == 1:
        angle_cnt_2_set = angle_cnt_2_set + 1
        
    #print(f"angle_cnt_2_set:{angle_cnt_2_set}")

    if t_forward_flag == True:
        Y_forward_flag = True
        # motor.control(0.15, 0)
        motor.control(motor_speed, motor_speed_base)
    else:
        Y_forward_flag = False
        # motor.control(0, 0.15)
        motor.control(motor_speed_base, motor_speed)

    
    # if angle == -45 or angle == 45:
    #     angle_cnt_2 = 0
    #     t_forward_flag = True
        
    #     # 计算与当前位置的角度差
    #     #print(f"current angle:{angle_cnt * 45}")
        
    #     if angle == -45:
    #         t_forward_flag = False
    #         t_cnt = -angle / 45
    #     elif angle == 45:
    #         t_forward_flag = True
    #         t_cnt = angle /45

    #     angle_cnt_2_set = t_cnt
    #     # 逆时针 0 1 0 1
    #     if angle < 0 and angle_check_flag == 0:
    #         angle_cnt_2_set = angle_cnt_2_set + 1
            
    #     # 顺时针 1 0 1 0
    #     if angle > 0 and angle_check_flag == 1:
    #         angle_cnt_2_set = angle_cnt_2_set + 1
         
    #     #print(f"angle_cnt_2_set:{angle_cnt_2_set}")

    #     if t_forward_flag == True:
    #         Y_forward_flag = True
    #         # motor.control(0.15, 0)
    #         motor.control(motor_speed, motor_speed_base)
    #     else:
    #         Y_forward_flag = False
    #         # motor.control(0, 0.15)
    #         motor.control(motor_speed_base, motor_speed)
        
    # else:
    #     print("angle is -180 or 180")
    #     return
    

def six_fangwei_cmd(cmd:str):
    """
    6 方位
    """

    global angle_cnt
    global angle_cnt_2
    global Y_forward_flag
    global wait_reset_flag
    global angle_check_flag


    if cmd == "CH":
        X_claw_control(0)
        Y_spin_control(0)
    elif cmd == "CB":
        X_claw_control(0)
        Y_spin_control(180) 
    elif cmd == "9H":
        X_claw_control(90)
        Y_spin_control(270) 
    elif cmd == "6H":
        X_claw_control(90)
        Y_spin_control(0) 
    elif cmd == "3H":
        X_claw_control(90)
        Y_spin_control(90) 
    elif cmd == "12H":
        X_claw_control(90)
        Y_spin_control(180)
    elif cmd == "20R":
        run_led.blink(0.1, 0.1, 1)
        return
    elif cmd == "20R0":
        run_led.off()
        return
    elif cmd == "20R1":
        run_led.on()
        return
    elif cmd == "21S":
        signal_led.blink(0.1, 0.1, 1)
        return
    elif cmd == "DH":
        X_claw_control(90)
    elif cmd == "UH":
        X_claw_control(0)
    elif cmd == "LH":
        angle_check_flag = angle_check.value
        print(f"angle_check state: {angle_check.value}")
        Y_spin_control_plus(-90)
        
        # 为了保证逆时针旋转是90度，需再顺时针旋转angle_check检测到低电平 进行位置修正
        # 等待电机停止运动
        # while motor.value != 0.0:
        #     time.sleep(0.01)
        # motor.control(motor_speed, motor_speed_base)
        # while angle_check.wait_for_inactive(0.01) != True:
        #     if angle_check.value == 0:
        #         motor.brake(keep_speed=1)
        
        # 测试逆时针旋转检测高电平停止
        # motor.control(motor_speed_base, motor_speed)
        # while angle_check.wait_for_active() != True:
        #     continue
        # motor.brake(keep_speed=1)
        
    elif cmd == "RH":
        angle_check_flag = angle_check.value
        #print(f"angle_check state: {angle_check.value}")
        Y_spin_control_plus(90)
    elif cmd == "RESET":
        # 逆时针旋转复位
        print("Y back to the origin")
        Y_forward_flag = False
        wait_reset_flag = False
        # motor.control(0, 0.2)
        motor.control(motor_speed_base, motor_speed)
        
        while wait_reset_flag == False:
            sleep(0.1)
        
        angle_cnt = 0
        angle_cnt_2 = 0

        # Y轴顺时针回原点    
        print("X back to the origin")
        sleep(0.5)
        X_claw_control(0)
    else:
        print(f"cmd error: {cmd}")
        return

    #print("wait Y motor stop...")
    # 等待电机停止运动
    while motor.value != 0.0:
        sleep(0.05)
    
    print("==================================")
    print("Y_motor stop!!!")


def get_mac_address(interface='eth0'):
    try:
        with open(f'/sys/class/net/{interface}/address', 'r') as f:
            mac_address = f.read().strip()
        return mac_address
    except Exception as e:
        return str(e)

def find_connected_display():
    drm_path = "/sys/class/drm"
    connected_devices = []
    for device in os.listdir(drm_path):
        status_path = os.path.join(drm_path, device, "status")
        if os.path.exists(status_path):
            with open(status_path, 'r') as status_file:
                status = status_file.read().strip()
                if status == "connected":
                    edid_path = os.path.join(drm_path, device, "edid")
                    if os.path.exists(edid_path):
                        connected_devices.append(edid_path)
    return connected_devices

def read_edid_file(edid_path):
    try:
        with open(edid_path, 'rb') as f:
            edid_data = f.read()
        return edid_data
    except FileNotFoundError:
        print(f"No EDID data found at {edid_path}")
        return None

def extract_hex_data(edid_data, start, end):
    if edid_data is None:
        return None

    data_slice = edid_data[start:end+1]
    hex_data = data_slice.hex()
    return hex_data


def md5_hash(text):
    # 对文本进行 MD5 加密
    hashed_text = hashlib.md5(text.encode()).hexdigest()
    return hashed_text

def read_saved_hash():
    # 读取保存的哈希值
    config = configparser.ConfigParser()
    config.read('/etc/motor_config.ini')
    
    if config.has_option('MAC', 'Hashed_MAC_Address'):
        print("Hashed MAC Address option found in the configuration file.")
        return config['MAC']['Hashed_MAC_Address']
    else:
        print("Hashed MAC Address option not found in the configuration file.")
        return False
    

def read_config_file(file_path):
    config = configparser.ConfigParser()
    try:
        with open(file_path, 'r') as f:
            # 添加错误检查，确保文件至少包含一个部分标题
            line = f.readline()
            if line.strip() == '':
                print("Configuration file contains no section headers.")
                return None
            f.seek(0)  # 将文件指针重新定位到文件开头
            config.read_file(f)
            return config
    except FileNotFoundError:
        print(f"Configuration file not found: {file_path}")
        return None
    except configparser.MissingSectionHeaderError:
        print("Configuration file contains no section headers.")
        return None

def MD5_check():    
    # 检查配置文件是否存在
    config_file = '/etc/motor_config.ini'
    if os.path.exists(config_file):
        config = read_config_file(config_file)
        if config:
            print("Configuration file loaded successfully.")
            # 在此处使用配置对象执行其他操作
            saved_hash = read_saved_hash()
            if saved_hash == False:
                return False
            
            is_first_time = False
        else:
            print("Failed to load configuration file.")
            return False    

    else:
        is_first_time = True

    # 获取树莓派的 MAC 地址
    mac_address = get_mac_address()

    # 对 MAC 地址进行 MD5 加密
    md5_result = md5_hash(mac_address)

    # 显示屏检测
    # connected_devices = find_connected_display()
    # if connected_devices:
    #     for edid_path in connected_devices:
    #         print(f"Found connected display EDID at: {edid_path}")
    #         edid_data = read_edid_file(edid_path)
    #         if edid_data:
    #             start_byte = 8
    #             end_byte = 15
    #             hex_data = extract_hex_data(edid_data, start_byte, end_byte)
    #             print(f"Hex data from byte {start_byte} to {end_byte}: {hex_data}")
                
    #             combined_data = mac_address + hex_data
    #             md5_result = md5_hash(combined_data)
    #             print(f"MD5 hash of combined data: {md5_result}")
    # else:
    #     print("No connected display found.")
    #     return False

    if is_first_time:
        # 第一次运行，保存哈希值到配置文件
        config = configparser.ConfigParser()
        config['MAC'] = {'Hashed_MAC_Address': md5_result}
        with open(config_file, 'w') as configfile:
            config.write(configfile)
        # print("第一次运行，已将哈希值保存到 config.ini 文件中。")
        return True
    else:
        # 比较保存的哈希值和当前计算的哈希值
        if saved_hash == md5_result:
            # print("MD5 值匹配，校验通过。")
            return True
        else:
            # print("MD5 值不匹配，校验失败。")
            return False          
            
def switch_to_text_console(console_number):
    # 拼接命令字符串
    command = "chvt {}".format(console_number)
    # 执行命令
    subprocess.run(command, shell=True)     

def reset_btn_handler():
    """_summary_
        复位按键
    """
    print("reset_btn_handler")
#    X_claw_control(0)
#    Y_spin_control(0)
    os.system("sudo /usr/bin/python3 /usr/local/bin/fifo_client.py RESET")    
    
def U_btn_press_handler():
    """_summary_
    五向按键:上
    """
    print("U_btn_press_handler")
    # X_claw_control(0)
    os.system("sudo /usr/bin/python3 /usr/local/bin/fifo_client.py UH")

def L_btn_press_handler():
    """_summary_
    五向按键:左
    """
    print("L_btn_press_handler")
    # Y_spin_control_plus(-180)
    os.system("sudo /usr/bin/python3 /usr/local/bin/fifo_client.py LH")

def D_btn_press_handler():
    """_summary_
    五向按键:下
    """
    print("D_btn_press_handler")
    # # X_claw_control(90)    
    os.system("sudo /usr/bin/python3 /usr/local/bin/fifo_client.py DH")

def M_btn_press_handler():
    """_summary_
    五向按键:中
    """
    print("M_btn_press_handler")

def R_btn_press_handler():
    """_summary_
    五向按键:右
    """
    print("R_btn_press_handler")
    # Y_spin_control_plus(180)
    os.system("sudo /usr/bin/python3 /usr/local/bin/fifo_client.py RH")


usb_audio_found = False

def usb_audio_scan():
    """_summary_
    检测 usb 音频设备是否插入
    """
    global usb_audio_found

    # 检查 USB 音频设备是否存在
    result = subprocess.run(['arecord', '-l'], capture_output=True, text=True)
    
    if "USB Audio" in result.stdout:
        if not usb_audio_found:
            print("signal blink cmd")
            signal_led.blink(on_time=0.1, off_time=0.1, n=None)
            
            usb_audio_found = True
    else:
        if usb_audio_found:
            signal_led.off()
            
            usb_audio_found = False

def read_sensor_status_thread(name):
    # 读取 sensor 值
    print(f"Thread {name}")
    while True:
        with open(bj_sensor_status_path, "w") as file:
                if sensor1.sensor.value:
                    file.write("1")
                    # print("Pin is HIGH")
                else:
                    file.write("0")
                    # print("Pin is LOW")
                
                if sensor2.sensor.value:
                    file.write("1")
                    # print("Pin is HIGH")
                else:
                    file.write("0")
                    # print("Pin is LOW")
                    
                if sensor3.sensor.value:
                    file.write("1")
                    # print("Pin is HIGH")
                else:
                    file.write("0")
                    # print("Pin is LOW")
                    
                if angle_check.value:
                    file.write("1")
                    # print("Pin is HIGH")
                else:
                    file.write("0")
                    # print("Pin is LOW")'
                    
                if input_device.value:
                    file.write("1")
                    # print("Pin is HIGH")
                else:
                    file.write("0")
                    # print("Pin is LOW")
        
        time.sleep(0.3)

if __name__ == '__main__':
    # # 获取树莓派的 MAC 地址
    # mac_address = get_mac_address()
    # # 对 MAC 地址进行 MD5 加密
    # md5_result = md5_hash(mac_address)
    # # 打印加密后的 MAC 地址
    # print("MAC 地址:", mac_address)
    # print("加密后的 MAC 地址 (MD5):", md5_result)

    if False == MD5_check():
        print("motor start check fail")
        # 切换到文本控制台7
        switch_to_text_console(7)
        sys.exit(-1)
    
    print("motor start success")
    
    # 创建线程
    thread = threading.Thread(target=read_sensor_status_thread, args=("Sensor_Status",))
    # 启动线程
    thread.start()
    
    # 注册回调函数，当 GPIO 引脚电平发生变化时调用
    input_device.when_activated = print_active_state
    input_device.when_deactivated = print_deactive_func
    
    angle_check.when_activated = angle_active_func
    angle_check.when_deactivated = angle_deactive_func

    
    print("motor start...")
    current_time = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
    print(f" {current_time} - Input state: {input_device.value}")
    
    
    sensor1.value_print()
    sensor2.value_print()
    sensor3.value_print()

    print(f"motor value:{motor.value}")

    # # 逆时针旋转复位
    print("Y back to the origin")
    Y_forward_flag = False
    # motor.control(0, 0.2)
    motor.control(motor_speed_base, motor_speed)
    
    while wait_reset_flag == False:
        sleep(0.1)
    
    angle_cnt = 0
    angle_cnt_2 = 0

    # Y轴顺时针回原点    
    print("X back to the origin")
    sleep(1)
    X_claw_control(0)

    angle_check_flag = angle_check.value
    #print(f"angle_check state: {angle_check.value}")
    
    # 按键触发
    reset_btn.when_deactivated = reset_btn_handler
    U_btn.when_deactivated = U_btn_press_handler
    L_btn.when_deactivated = L_btn_press_handler
    D_btn.when_deactivated = D_btn_press_handler
    M_btn.when_deactivated = M_btn_press_handler
    R_btn.when_deactivated = R_btn_press_handler


    print("motor init success!!!")
    print("Version:", VERSION)

    # 初始化命名管道
    if os.path.exists(read_path):
        os.remove(read_path)
    if os.path.exists(write_path):
        os.remove(write_path)

    os.mkfifo(write_path)
    os.mkfifo(read_path)
    
    # 音频输入检测
    usb_audio_scan()

    rf = os.open(read_path, os.O_RDONLY | os.O_NONBLOCK)
    # wf = os.open(write_path, os.O_SYNC | os.O_CREAT | os.O_RDWR)

    # 程序保持运行状态，直到手动中断
    # pause()
    
    print("rf ready")
    os.makedirs(os.path.dirname(bj_sensor_status_path), exist_ok=True)
    
    # 清除缓存
    # bytes.decode(os.read(rf, 1024))
    
    # 6方位控制
    while True:
        # str = input("input cmd >")
        # print("cmd > ", str)
        # six_fangwei_cmd(str)
        
        # 1. 音频输入检测
        usb_audio_scan()
   
        rlist, _, _ = select.select([rf], [], [])     
        if rf in rlist:
            cmd = bytes.decode(os.read(rf, 1024))
            if len(cmd) == 0:
                time.sleep(0.05)
                continue
        
            print(f"received cmd: {cmd}, type:", type(cmd))
            six_fangwei_cmd(cmd)
            bytes.decode(os.read(rf, 1024))
            
        
# """     
#         cmd = bytes.decode(os.read(rf, 1024))
#         if len(cmd) == 0:
#             time.sleep(0.05)
#             continue
        
#         print(f"received cmd: {cmd}, type:", type(cmd))
#         six_fangwei_cmd(cmd)
#         bytes.decode(os.read(rf, 1024))
# """
     
    os.close(rf)
    # os.close(wf)
    
    # Y轴向旋转控制
    # while True:
    #     str = input("input angle>")
    #     print("angle>", str)
    #     if str.isdigit() != True:
    #         print(f"input need number, error input:{str}")
    #         continue
        
    #     Y_spin_control(int(str))

    #     # 等待电机停止运动
    #     while motor.value != 0.0:
    #         sleep(0.5)
            
    #     print("Y_motor stop!!!")
    
    # X爪子旋转控制
    # while True:
    #     str = input("claw_X - input angle>")
    #     angle = int(str)
    #     print("angle>>", angle)
        
    #     X_claw_control(angle)
    #     sleep(1)



